
#include "Math3D.h"

namespace ors
{

	//! copy operator
	/*
	Vector& Vector::operator=(const Vector& b){
	v[0]=b.v[0]; v[1]=b.v[1]; v[2]=b.v[2]; return *this;
	}*/

	//! copy operator
	/*Vector& Vector::operator=(const double* b){
	v[0]=b[0]; v[1]=b[1]; v[2]=b[2]; return *this;
	}*/

	//! set all entries to same value
	/*Vector& Vector::operator=(double b){
	v[0]=v[1]=v[2]=b; return *this;
	}*/


	//{ access
	//! lhs reference
	double& Vector::operator()(int i){ CHECK(i>=0 && i<3,"ors::Vector access - out of range"); return p[i]; }
	const double& Vector::operator()(int i) const{ CHECK(i>=0 && i<3,"ors::Vector access - out of range"); return p[i]; }
	//double& Vector::operator[](int i){ CHECK(i>=0 && i<3,"ors::Vector access - out of range"); return v[i]; }

#ifdef MT_MSVC
	//! double-pointer access
	//Vector::operator double*(){ return v; }
#endif

	//! double-pointer access
	//Vector::operator const double*() const{ return v; }

	//! set the vector
	void Vector::set(double x,double y,double z){ p[0]=x; p[1]=y; p[2]=z; }

	//! set the vector
	void Vector::set(double* x){ p[0]=x[0]; p[1]=x[1]; p[2]=x[2]; }

	//! set the vector
	void Vector::setZero(){ p[0]=p[1]=p[2]=0.; }

	//! a random vector in [-1,1]^3
	void Vector::setRandom(){ p[0]=rnd.uni(-1,1); p[1]=rnd.uni(-1,1); p[2]=rnd.uni(-1,1); }

	//{ vector operations

	/*
	void Vector::multiply(double c){
	v[0]*=c; v[1]*=c; v[2]*=c;
	}

	void Vector::divide(double c){
	v[0]/=c; v[1]/=c; v[2]/=c;
	}*/

	//! this=this+b
	void Vector::add(double x,double y,double z){ p[0]+=x; p[1]+=y; p[2]+=z; }

	//! this=this-b
	void Vector::subtract(double x,double y,double z){ p[0]-=x; p[1]-=y; p[2]-=z; }

	//! this=this/length(this)
	void Vector::normalize(){ (*this)/=length(); }

	//! this=this*l/length(this)
	void Vector::setLength(double l){
		if(isZero()) MT_MSG("can't change length of null vector");
		(*this)*=l/length();
	}

	//! this=component of this normal to \c b, (unnormalized!)
	void Vector::makeNormal(const Vector& b){
		double l=b.length(),s=p[0]*b.p[0]+p[1]*b.p[1]+p[2]*b.p[2];
		s/=l*l;
		p[0]-=s*b.p[0]; p[1]-=s*b.p[1]; p[2]-=s*b.p[2];
	}

	//! this=component of this colinear to \c b, (unnormalized!)
	void Vector::makeColinear(const Vector& b){
		// *this = ((*this)*b)/b.length()) * (*this);
		double l=b.length(),s=p[0]*b.p[0]+p[1]*b.p[1]+p[2]*b.p[2];
		s/=l*l;
		p[0]=s*b.p[0]; p[1]=s*b.p[1]; p[2]=s*b.p[2];
	}

	//{ measuring the vector

	//! is zero?
	bool Vector::isZero() const{ return (p[0]==0. && p[1]==0. && p[2]==0.); }

	//! is it normalized?
	bool Vector::isNormalized() const{ return fabs(lengthSqr()-1.)<1e-6; }

	//! returns the length of this
	double Vector::length() const{ return ::sqrt(lengthSqr()); }

	//! returns the square of length |a|^2
	double Vector::lengthSqr() const{ return p[0]*p[0] + p[1]*p[1] + p[2]*p[2]; }

	//! angle in [0..pi] between this and b
	double Vector::angle(const Vector& b) const{
		double x=((*this)*b)/(length()*b.length());
		if(x<-1.) x=-1.;
		if(x>1.) x=1.;
		return ::acos(x);
	}

	/*!\brief if \c this and \c b are colinear, it returns the factor c such
	that this=c*b; otherwise it returns zero */
	double Vector::isColinear(const Vector& b) const{
		double c=p[0]/b.p[0];
		if(p[1]==c*b.p[1] && p[2]==c*b.p[2]) return c;
		return 0.;
	}


	//{ sphere coordinates

	//! the radius in the x/y-plane
	double Vector::radius() const{ return ::sqrt(p[0]*p[0]+p[1]*p[1]); }
	//! the angle in the x/y-plane in [-pi, pi]
	double Vector::phi() const{
		double ph;
		if(p[0]==0. || ::fabs(p[0])<1e-10) ph=MT_PI/2.; else ph=::atan(p[1]/p[0]);
		if(p[0]<0.){ if(p[1]<0.) ph-=MT_PI; else ph+=MT_PI; }
		return ph;
	}
	//! the angle from the x/y-plane
	double Vector::theta() const{ return ::atan(p[2]/radius())+MT_PI/2.; }


	//{ I/O
	void Vector::write(std::ostream& os) const{
		if(!MT::IOraw) os <<'(' <<p[0] <<' ' <<p[1] <<' ' <<p[2] <<')';
		else os <<' ' <<p[0] <<' ' <<p[1] <<' ' <<p[2];
	}
	void Vector::read(std::istream& is){
		if(!MT::IOraw) is >>"(" >>p[0] >>p[1] >>p[2] >>")";
		else is >>p[0] >>p[1] >>p[2];
	}
	//}

	//! scalar product (inner product)
	double operator*(const Vector& a,const Vector& b){
		return a.p[0]*b.p[0]+a.p[1]*b.p[1]+a.p[2]*b.p[2];
	}

	//! cross product (corresponds to antisymmetric exterior product)
	Vector operator^(const Vector& b,const Vector& c){
		Vector a;
		a.p[0]=b.p[1]*c.p[2]-b.p[2]*c.p[1];
		a.p[1]=b.p[2]*c.p[0]-b.p[0]*c.p[2];
		a.p[2]=b.p[0]*c.p[1]-b.p[1]*c.p[0];
		return a;
	}

	//! sum of two vectors 
	Vector operator+(const Vector& b,const Vector& c){
		Vector a;
		a.p[0]=b.p[0]+c.p[0];
		a.p[1]=b.p[1]+c.p[1];
		a.p[2]=b.p[2]+c.p[2];
		return a;
	}

	//! difference between two vectors
	Vector operator-(const Vector& b,const Vector& c){
		Vector a;
		a.p[0]=b.p[0]-c.p[0];
		a.p[1]=b.p[1]-c.p[1];
		a.p[2]=b.p[2]-c.p[2];
		return a;
	}

	//! multiplication with a scalar
	Vector operator*(double b,const Vector& c){
		Vector a;
		a.p[0]=b*c.p[0];
		a.p[1]=b*c.p[1];
		a.p[2]=b*c.p[2];
		return a;
	}

	//! multiplication with a scalar
	Vector operator*(const Vector& b,double c){ return c*b; }

	//! division by a scalar
	Vector operator/(const Vector& b,double c){ return (1./c)*b; }

	//! multiplication with a scalar
	Vector& operator*=(Vector& a,double c){
		a.p[0]*=c; a.p[1]*=c; a.p[2]*=c;
		return a;
	}
	//! divide by a scalar
	Vector& operator/=(Vector& a,double c){
		a.p[0]/=c; a.p[1]/=c; a.p[2]/=c;
		return a;
	}
	//! add a vector
	Vector& operator+=(Vector& a,const Vector& b){
		a.p[0]+=b.p[0]; a.p[1]+=b.p[1]; a.p[2]+=b.p[2];
		return a;
	}
	//! subtract a vector
	Vector& operator-=(Vector& a,const Vector& b){
		a.p[0]-=b.p[0]; a.p[1]-=b.p[1]; a.p[2]-=b.p[2];
		return a;
	}
	//! return the negative of a vector
	Vector operator-(const Vector& b){
		Vector a;
		a.p[0]=-b.p[0]; a.p[1]=-b.p[1]; a.p[2]=-b.p[2];
		return a;
	}

	//! const access via two row and column indices
	const double& Matrix::operator()(int i,int j) const{ return p[i*3+j]; }
	//! LHS access via two row and column indices
	double& Matrix::operator()(int i,int j){ return p[i*3+j]; }

	//! copy operator
	/*Matrix& Matrix::operator=(const Matrix& b){
	memmove(m,b.m,9*sizeof(double)); return *this;
	}*/

	//! reset to zero
	void Matrix::setZero(){
		p[0]=p[4]=p[8]=0.;
		p[1]=p[2]=p[3]=p[5]=p[6]=p[7]=0.;
	}

	//! reset to identity
	void Matrix::setId(){
		p[0]=p[4]=p[8]=1.;
		p[1]=p[2]=p[3]=p[5]=p[6]=p[7]=0.;
	}

	//! assign the matrix to the transformation from unit frame to given XYZ frame
	void Matrix::setFrame(Vector& X,Vector& Y,Vector& Z){
		p[0]=X(0); p[1]=Y(0); p[2]=Z(0);
		p[3]=X(1); p[4]=Y(1); p[5]=Z(1);
		p[6]=X(2); p[7]=Y(2); p[8]=Z(2);
	}

	//! assign the matrix to the transformation from the ORTHOGONAL XYZ frame to the unit frame
	void Matrix::setInvFrame(Vector& X,Vector& Y,Vector& Z){
		p[0]=X(0); p[1]=X(1); p[2]=X(2);
		p[3]=Y(0); p[4]=Y(1); p[5]=Y(2);
		p[6]=Z(0); p[7]=Z(1); p[8]=Z(2);
	}

	//! assign the matrix to a rotation around the X-axis with angle a (in rad units)
	void Matrix::setXrot(double a){
		p[0]=1.; p[1]=0.;     p[2]=0.;
		p[3]=0.; p[4]=cos(a); p[5]=-sin(a);
		p[6]=0.; p[7]=sin(a); p[8]= cos(a);
	}

	void Matrix::setSkew(const Vector& a){
		p[0]=   0.; p[1]=-a(2); p[2]= a(1);
		p[3]= a(2); p[4]=   0.; p[5]=-a(0);
		p[6]=-a(1); p[7]= a(0); p[8]=   0.;
	}

	void Matrix::setExponential(const Vector& a){
		Matrix S;
		double phi=a.length();
		if(phi<1e-10){ setId(); return; }
		S.setSkew(a/phi);
		*this = sin(phi)*S + (1.-cos(phi))*S*S;
		p[0]+=1.; p[4]+=1.; p[8]+=1.;
	}

	void Matrix::setOdeMatrix(double* o){
		p[0]=o[0]; p[1]=o[1]; p[2]=o[2];
		p[3]=o[4]; p[4]=o[5]; p[5]=o[6];
		p[6]=o[8]; p[7]=o[9]; p[8]=o[10];
	}

	void Matrix::setTensorProduct(const Vector& b,const Vector& c){
		p[0]=b.p[0]*c.p[0]; p[1]=b.p[0]*c.p[1]; p[2]=b.p[0]*c.p[2];
		p[3]=b.p[1]*c.p[0]; p[4]=b.p[1]*c.p[1]; p[5]=b.p[1]*c.p[2];
		p[6]=b.p[2]*c.p[0]; p[7]=b.p[2]*c.p[1]; p[8]=b.p[2]*c.p[2];
	}

	void Matrix::write(std::ostream& os) const{
		os <<"\n " <<p[0] <<' ' <<p[1] <<' ' <<p[2];
		os <<"\n " <<p[3] <<' ' <<p[4] <<' ' <<p[5];
		os <<"\n " <<p[6] <<' ' <<p[7] <<' ' <<p[8];
		os <<endl;
	}
	void Matrix::read(std::istream& is){
		NIY;
	}
	//}

	//! multiplication of two matrices
	Matrix operator*(const Matrix& b,const Matrix& c){
		Matrix a;
		a(0,0)=b(0,0)*c(0,0)+b(0,1)*c(1,0)+b(0,2)*c(2,0);
		a(0,1)=b(0,0)*c(0,1)+b(0,1)*c(1,1)+b(0,2)*c(2,1);
		a(0,2)=b(0,0)*c(0,2)+b(0,1)*c(1,2)+b(0,2)*c(2,2);

		a(1,0)=b(1,0)*c(0,0)+b(1,1)*c(1,0)+b(1,2)*c(2,0);
		a(1,1)=b(1,0)*c(0,1)+b(1,1)*c(1,1)+b(1,2)*c(2,1);
		a(1,2)=b(1,0)*c(0,2)+b(1,1)*c(1,2)+b(1,2)*c(2,2);

		a(2,0)=b(2,0)*c(0,0)+b(2,1)*c(1,0)+b(2,2)*c(2,0);
		a(2,1)=b(2,0)*c(0,1)+b(2,1)*c(1,1)+b(2,2)*c(2,1);
		a(2,2)=b(2,0)*c(0,2)+b(2,1)*c(1,2)+b(2,2)*c(2,2);
		return a;
	}
	//! sum of two matrices
	Matrix operator+(const Matrix& b,const Matrix& c){
		Matrix a;
		a.p[0]=b.p[0]+c.p[0]; a.p[1]=b.p[1]+c.p[1]; a.p[2]=b.p[2]+c.p[2];
		a.p[3]=b.p[3]+c.p[3]; a.p[4]=b.p[4]+c.p[4]; a.p[5]=b.p[5]+c.p[5];
		a.p[6]=b.p[6]+c.p[6]; a.p[7]=b.p[7]+c.p[7]; a.p[8]=b.p[8]+c.p[8];
		return a;
	}
	//! transformation of a vector
	Vector operator*(const Matrix& b,const Vector& c){
		Vector a;
		a.p[0]=b.p[0]*c.p[0]+b.p[1]*c.p[1]+b.p[2]*c.p[2];
		a.p[1]=b.p[3]*c.p[0]+b.p[4]*c.p[1]+b.p[5]*c.p[2];
		a.p[2]=b.p[6]*c.p[0]+b.p[7]*c.p[1]+b.p[8]*c.p[2];
		return a;
	}
	//! multiplication with a scalar
	Matrix& operator*=(Matrix& a,double c){
		a.p[0]*=c; a.p[1]*=c; a.p[2]*=c;
		a.p[3]*=c; a.p[4]*=c; a.p[5]*=c;
		a.p[6]*=c; a.p[7]*=c; a.p[8]*=c;
		return a;
	}
	//! multiplication with scalar
	Matrix operator*(double b,const Matrix& c){
		Matrix a;
		a=c;
		a*=b;
		return a;
	}
	//! sum of two matrices
	Matrix& operator+=(Matrix& a,const Matrix& b){
		a.p[0]+=b.p[0]; a.p[1]+=b.p[1]; a.p[2]+=b.p[2];
		a.p[3]+=b.p[3]; a.p[4]+=b.p[4]; a.p[5]+=b.p[5];
		a.p[6]+=b.p[6]; a.p[7]+=b.p[7]; a.p[8]+=b.p[8];
		return a;
	}

	//! initializes to identity
	Quaternion::Quaternion(){
		setZero();
	}

	//! inverts the current rotation
	void Quaternion::invert(){ p[0]=-p[0]; }

	//! multiplies the rotation by a factor f (i.e., makes f-times the rotation)
	void Quaternion::multiply(double f){
		if(p[0]==1. || f==1.) return;
		double phi=acos(p[0]);
		phi*=f;
		p[0]=cos(phi);
		f=sin(phi)/sqrt(p[1]*p[1] + p[2]*p[2] + p[3]*p[3]);
		p[1]*=f; p[2]*=f; p[3]*=f;
	}

	bool Quaternion::isNormalized() const{
		double n=p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3];
		return fabs(n-1.)<1e-6;
	}

	void Quaternion::normalize(){
		double n=p[0]*p[0] + p[1]*p[1] + p[2]*p[2] + p[3]*p[3];
		n=sqrt(n);
		p[0]/=n; p[1]/=n; p[2]/=n; p[3]/=n;
	}

	/*!\brief roughly, removes all ``components'' of the rotation that are not
	around the given vector v. More precisely, aligns/projects
	the rotation axis (given by q[1],q[2],q[3] of the quaternion)
	with v and re-normalizes afterwards. */
	void Quaternion::alignWith(const Vector& v){
		double s=p[1]*v(0) + p[2]*v(1) + p[3]*v(2);
		if(!s){ setZero(); return; }// are orthogonal
		s/=v*v;
		p[1]=s*v(0); p[2]=s*v(1); p[3]=s*v(2);
		normalize();
	}


	//! set the quad
	void Quaternion::set(double* x){ p[0]=x[0]; p[1]=x[1]; p[2]=x[2]; p[3]=x[3]; }
	//! set the quad
	void Quaternion::set(double q0,double x,double y,double z){ p[0]=q0; p[1]=x; p[2]=y; p[3]=z; }
	//! reset the rotation to identity
	void Quaternion::setZero(){ p[0]=1; p[1]=p[2]=p[3]=0; }
	//! samples the rotation uniformly from the whole SO(3)
	void Quaternion::setRandom(){
		double s,s1,s2,t1,t2;
		s=rnd.uni();
		s1=sqrt(1-s);
		s2=sqrt(s);
		t1=MT_2PI*rnd.uni();
		t2=MT_2PI*rnd.uni();
		p[0]=cos(t2)*s2;
		p[1]=sin(t1)*s1;
		p[2]=cos(t1)*s1;
		p[3]=sin(t2)*s2;
	}

	//! sets this to a smooth interpolation between two rotations
	void Quaternion::setInterpolate(double t,const Quaternion& a,const Quaternion b){
		double sign=1.;
		if(scalarProduct(a,b)<0) sign=-1.;
		p[0]=a.p[0]+t*(sign*b.p[0]-a.p[0]);
		p[1]=a.p[1]+t*(sign*b.p[1]-a.p[1]);
		p[2]=a.p[2]+t*(sign*b.p[2]-a.p[2]);
		p[3]=a.p[3]+t*(sign*b.p[3]-a.p[3]);
		normalize();
	}

	//! assigns the rotation to \c a DEGREES around the vector (x,y,z)
	void Quaternion::setDeg(double degree,double x,double y,double z){ setRad(degree*MT_PI/180.,x,y,z); }
	void Quaternion::setDeg(double degree,const Vector& vec){ setRad(degree*MT_PI/180.,vec(0),vec(1),vec(2)); }
	//! assigns the rotation to \c a RADIANTS (2*PI-units) around the vector (x,y,z)
	void Quaternion::setRad(double angle,double x,double y,double z){
		double l = x*x + y*y + z*z;
		if(l<1e-15){ setZero(); return; }
		angle/=2.;
		l=sin(angle)/sqrt(l);
		p[0]=cos(angle);
		p[1]=x*l;
		p[2]=y*l;
		p[3]=z*l;
	}
	//! ..
	void Quaternion::setRad(double angle,const Vector &axis){
		setRad(angle,axis(0),axis(1),axis(2));
	}
	//! assigns the rotation to \c a RADIANTS (2*PI-units) around the current axis
	void Quaternion::setRad(double angle){
		double l = p[1]*p[1] + p[2]*p[2] + p[3]*p[3];
		if(l<1e-15){ setZero(); return; }
		angle/=2.;
		l=sin(angle)/sqrt(l);
		p[0]=cos(angle);
		p[1]*=l;
		p[2]*=l;
		p[3]*=l;
	}
	//! rotation around X-axis by given radiants
	void Quaternion::setRadX(double angle){
		angle/=2.;
		p[0]=cos(angle);
		p[1]=sin(angle);
		p[2]=p[3]=0.;
	}
	//! rotation around X-axis by given radiants
	void Quaternion::setRadY(double angle){
		angle/=2.;
		p[0]=cos(angle);
		p[2]=sin(angle);
		p[1]=p[3]=0.;
	}
	//! rotation around the given vector with angle (in rad) equal to norm of the vector
	void Quaternion::setVec(Vector w){
		double phi=w.length();
		setRad(phi,w(0),w(1),w(2));
	}
	//! rotation that will rotate 'from' to 'to' on direct path
	void Quaternion::setDiff(const Vector& from,const Vector& to){
		double phi=acos(from*to/(from.length()*to.length()));
		if(!phi) return;
		Vector axis(from^to);
		if(axis.isZero()) axis=Vector(0,0,1)^to;
		setRad(phi,axis);
	}

	//! is identical
	bool Quaternion::isZero() const{ return p[0]==1.; }

#ifdef MT_MSVC
	//! double-pointer access
	//Quaternion::operator double*(){ return q; }
#endif

	//! double-pointer access
	//Quaternion::operator const double*() const{ return q; }

	//! gets rotation angle (in rad [0,2pi])
	double Quaternion::getRad() const{
		if(p[0]>=1. || p[0]<=-1. || (p[1]==0. && p[2]==0. && p[3]==0.)) return 0;
		return 2.*acos(p[0]);
	}

	//! gets rotation angle (in degree [0,360])
	double Quaternion::getDeg() const{
		if(p[0]>=1. || p[0]<=-1. || (p[1]==0. && p[2]==0. && p[3]==0.)) return 0;
		return 360./MT_PI*acos(p[0]);
	}

	//! gets rotation angle (in degree [0,360]) and vector
	void Quaternion::getDeg(double& degree,Vector& vec) const{
		if(p[0]>=1. || p[0]<=-1. || (p[1]==0. && p[2]==0. && p[3]==0.)){ degree=0.; vec.set(0.,0.,1.); return; }
		degree=acos(p[0]);
		double s=sin(degree);
		degree*=360./MT_PI;
		vec(0)=p[1]/s; vec(1)=p[2]/s; vec(2)=p[3]/s;
	}

	//! gets rotation angle (in rad [0,2pi]) and vector
	void Quaternion::getRad(double& angle,Vector& vec) const{
		if(p[0]>=1. || p[0]<=-1. || (p[1]==0. && p[2]==0. && p[3]==0.)){ angle=0.; vec.set(0.,0.,1.); return; }
		angle=acos(p[0]);
		double s=sin(angle);
		angle*=2;
		vec(0)=p[1]/s; vec(1)=p[2]/s; vec(2)=p[3]/s;
		CHECK(angle>=0. && angle<=MT_2PI,"");
	}

	//! gets the axis rotation vector with length equal to the rotation angle in rad
	Vector& Quaternion::getVec(Vector& v) const{
		if(p[0]>=1. || p[0]<=-1. || (p[1]==0. && p[2]==0. && p[3]==0.)){ v.setZero(); return v; }
		double phi=acos(p[0]);
		double s=2.*phi/sin(phi);
		v(0)=s*p[1]; v(1)=s*p[2]; v(2)=s*p[3];
		return v;
	}

	Vector& Quaternion::getX(Vector& Rx) const{
		double q22 = 2.*p[2]*p[2];
		double q33 = 2.*p[3]*p[3];
		double q12 = 2.*p[1]*p[2];
		double q13 = 2.*p[1]*p[3];
		double q02 = 2.*p[0]*p[2];
		double q03 = 2.*p[0]*p[3];
		Rx.p[0]=1-q22-q33;
		Rx.p[1]=q12+q03;
		Rx.p[2]=q13-q02;
		return Rx;
	}
	Vector& Quaternion::getY(Vector& Ry) const{ Ry = (*this)*Vector(0,1,0);  return Ry; }
	Vector& Quaternion::getZ(Vector& Rz) const{ Rz = (*this)*Vector(0,0,1);  return Rz; }

	void Quaternion::setMatrix(double* m){
		p[0] = .5*sqrt(1.+m[0]+m[4]+m[8]); //sqrt(1.-(3.-(m[0]+m[4]+m[8]))/4.);
		p[3] = (m[3]-m[1])/(4.*p[0]);
		p[2] = (m[2]-m[6])/(4.*p[0]);
		p[1] = (m[7]-m[5])/(4.*p[0]);
		normalize();
		//CHECK(normalized(),"failed :-(");
	}

	//! exports the rotation to a double[9] matrix, row-by-row
	double* Quaternion::getMatrix(double* m) const{
		double P1=2.*p[1], P2=2.*p[2], P3=2.*p[3];
		double q11 = p[1]*P1;
		double q22 = p[2]*P2;
		double q33 = p[3]*P3;
		double q12 = p[1]*P2;
		double q13 = p[1]*P3;
		double q23 = p[2]*P3;
		double q01 = p[0]*P1;
		double q02 = p[0]*P2;
		double q03 = p[0]*P3;
		m[0]=1.-q22-q33; m[1]=q12-q03;    m[2]=q13+q02;
		m[3]=q12+q03;   m[4]=1.-q11-q33;  m[5]=q23-q01;
		m[6]=q13-q02;   m[7]=q23+q01;    m[8]=1.-q11-q22;
		return m;
	}

	//! exports the rotation to an ODE format matrix of type double[12]
	double* Quaternion::getMatrixOde(double* m) const{
		double P1=2.*p[1], P2=2.*p[2], P3=2.*p[3];
		double q11 = p[1]*P1;
		double q22 = p[2]*P2;
		double q33 = p[3]*P3;
		double q12 = p[1]*P2;
		double q13 = p[1]*P3;
		double q23 = p[2]*P3;
		double q01 = p[0]*P1;
		double q02 = p[0]*P2;
		double q03 = p[0]*P3;
		m[0]=1.-q22-q33; m[1]=q12-q03;   m[2] =q13+q02;
		m[4]=q12+q03;   m[5]=1.-q11-q33; m[6] =q23-q01;
		m[8]=q13-q02;   m[9]=q23+q01;   m[10]=1.-q11-q22;
		m[3]=m[7]=m[11]=0.;
		return m;
	}
	//! exports the rotation to an OpenGL format matrix of type double[16]
	double* Quaternion::getMatrixGL(double* m) const{
		double P1=2.*p[1], P2=2.*p[2], P3=2.*p[3];
		double q11 = p[1]*P1;
		double q22 = p[2]*P2;
		double q33 = p[3]*P3;
		double q12 = p[1]*P2;
		double q13 = p[1]*P3;
		double q23 = p[2]*P3;
		double q01 = p[0]*P1;
		double q02 = p[0]*P2;
		double q03 = p[0]*P3;
		m[0]=1.-q22-q33; m[4]=q12-q03;  m[8] =q13+q02;
		m[1]=q12+q03;   m[5]=1.-q11-q33; m[9] =q23-q01;
		m[2]=q13-q02;   m[6]=q23+q01;  m[10]=1.-q11-q22;
		m[3]=m[7]=m[11]=m[12]=m[13]=m[14]=0.;
		m[15]=1.;
		return m;
	}

	void Quaternion::writeNice(std::ostream& os) const{ Vector v; os <<"Quaternion: " <<getDeg() <<" around " <<getVec(v) <<"\n"; }
	void Quaternion::write(std::ostream& os) const{
		if(!MT::IOraw) os <<'(' <<p[0] <<' ' <<p[1] <<' ' <<p[2] <<' ' <<p[3] <<')';
		else os <<' ' <<p[0] <<' ' <<p[1] <<' ' <<p[2] <<' ' <<p[3];
	}
	void Quaternion::read(std::istream& is){ is >>"(" >>p[0] >>p[1] >>p[2]  >>p[3] >>")"; normalize();}
	//}

	//! compound of two rotations (A=B*C)
	Quaternion operator*(const Quaternion& b,const Quaternion& c){
		Quaternion a;
		a.p[0] = b.p[0]*c.p[0] - b.p[1]*c.p[1] - b.p[2]*c.p[2] - b.p[3]*c.p[3];
		a.p[1] = b.p[0]*c.p[1] + b.p[1]*c.p[0] + b.p[2]*c.p[3] - b.p[3]*c.p[2];
		a.p[2] = b.p[0]*c.p[2] + b.p[2]*c.p[0] + b.p[3]*c.p[1] - b.p[1]*c.p[3];
		a.p[3] = b.p[0]*c.p[3] + b.p[3]*c.p[0] + b.p[1]*c.p[2] - b.p[2]*c.p[1];
		return a;
	}

	//! A=B*C^{-1}
	Quaternion operator/(const Quaternion& b,const Quaternion& c){
		Quaternion a;
		a.p[0] =-b.p[0]*c.p[0] - b.p[1]*c.p[1] - b.p[2]*c.p[2] - b.p[3]*c.p[3];
		a.p[1] = b.p[0]*c.p[1] - b.p[1]*c.p[0] + b.p[2]*c.p[3] - b.p[3]*c.p[2];
		a.p[2] = b.p[0]*c.p[2] - b.p[2]*c.p[0] + b.p[3]*c.p[1] - b.p[1]*c.p[3];
		a.p[3] = b.p[0]*c.p[3] - b.p[3]*c.p[0] + b.p[1]*c.p[2] - b.p[2]*c.p[1];
		return a;
	}

	//! transform of a vector by a rotation
	Vector operator*(const Quaternion& b,const Vector& c){
		double m[9];
		b.getMatrix(m);
		Vector a;
		a.p[0]=m[0]*c.p[0]+m[1]*c.p[1]+m[2]*c.p[2];
		a.p[1]=m[3]*c.p[0]+m[4]*c.p[1]+m[5]*c.p[2];
		a.p[2]=m[6]*c.p[0]+m[7]*c.p[1]+m[8]*c.p[2];
		return a;
	}

	//! inverse transform of a vector by a rotation
	Vector operator/(const Quaternion& b,const Vector& c){
		double m[9];
		b.getMatrix(m);
		Vector a;
		a.p[0]=m[0]*c.p[0]+m[3]*c.p[1]+m[6]*c.p[2];
		a.p[1]=m[1]*c.p[0]+m[4]*c.p[1]+m[7]*c.p[2];
		a.p[2]=m[2]*c.p[0]+m[5]*c.p[1]+m[8]*c.p[2];
		return a;
	}

	//! transform of a vector by a frame
	Vector operator*(const Transformation& X,const Vector& c){
		Vector a;
		a = X.rot * c;
		a += X.pos;
		return a;
	}

	//! inverse transform of a vector by a frame
	Vector operator/(const Transformation& X,const Vector& c){
		Vector a(c);
		a -= X.pos;
		a = X.rot / a;
		return a;
	}

	//! constructor
	Transformation::Transformation(){ setZero(); }

	//! copy operator
	//Transformation& Transformation::operator=(const Transformation& f){
	//  p=f.p; v=f.v; r=f.r; w=f.w; /*a=f.a; b=f.b; s=f.s;*/ return *this; }

	//! copy operator
	//Transformation& Transformation::set(const Transformation& f){
	//p=f.p; v=f.v; r=f.r; w=f.w; /*a=f.a; b=f.b; s=f.s;*/ return *this; }

	//! initialize by reading from the string
	Transformation& Transformation::setText(const char* txt){ read(MT::String(txt)()); return *this; }

	//! initialize by reading from the stream
	//Transformation& Transformation::set(istream &is){ setZero(); read(is); return *this; }

	//! resets the position to origin, rotation to identity, velocities to zero, scale to unit
	void Transformation::setZero(){
		pos.setZero(); vel.setZero(); rot.setZero(); angvel.setZero(); /*a.setZero(); b.setZero(); s=1.;*/ }

	//! randomize the frame
	void Transformation::setRandom(){
		pos.setRandom(); vel.setRandom(); rot.setRandom(); angvel.setRandom(); /*a.setRandom(); b.setRandom(); s=rnd.uni();*/ }

	/*!\brief moves the frame according to the current velocities \c v and \c w
	and the time span \c time (if time is given in seconds, v has
	dimension units/sec, and w has dimension rad/sec) */
	/*void Transformation::step(double time){
	Quaternion W;
	W.setVec(w);
	W.multiply(time);
	p+=v;
	r=W*r;
	}*/

	//! multiply the current scale by f
	/*void Transformation::scale(double f){
	s*=f;
	}*/
	//! move the turtle by the vector (x,z,y) WITH RESPECT TO the current orientation/scale
	void Transformation::addRelativeTranslation(double x,double y,double z){
		Vector X(x,y,z);
		//X=r*(s*X); //in global coords
		X=rot*X; //in global coords
		pos+=X;
		vel+=angvel^X;
	}
	//! add a velocity to the turtle's inertial frame
	void Transformation::addRelativeVelocity(double x,double y,double z){
		Vector X(x,y,z);
		//v+=r*(s*X);
		vel+=rot*X;
	}
	//! add an angular velocity to the turtle inertial frame
	void Transformation::addRelativeAngVelocityDeg(double degree,double x,double y,double z){
		Vector W(x,y,z); W.normalize();
		W*=degree*MT_PI/180.;
		angvel+=rot*W;
	}
	//! add an angular velocity to the turtle inertial frame
	void Transformation::addRelativeAngVelocityRad(double rad,double x,double y,double z){
		Vector W(x,y,z); W.normalize();
		W*=rad;
		angvel+=rot*W;
	}
	//! add an angular velocity to the turtle inertial frame
	void Transformation::addRelativeAngVelocityRad(double wx,double wy,double wz){
		Vector W(wx,wy,wz);
		angvel+=rot*W;
	}
	//! rotate the turtle orientation by an angle (given in DEGREE) around the vector (x,y,z) (given relative to the current orientation)
	void Transformation::addRelativeRotationDeg(double degree,double x,double y,double z){
		Quaternion R;
		R.setDeg(degree,x,y,z);
		rot=rot*R;
	}
	//! rotate the turtle orientation by an angle (given in radiants) around the vector (x,y,z) (given relative to the current orientation)
	void Transformation::addRelativeRotationRad(double rad,double x,double y,double z){
		Quaternion R;
		R.setRad(rad,x,y,z);
		rot=rot*R;
	}
	//! rotate the turtle orientation as given by a quaternion
	void Transformation::addRelativeRotationQuat(double s,double x,double y,double z){
		Quaternion R;
		R.p[0]=s; R.p[1]=x; R.p[2]=y; R.p[3]=z;
		rot=rot*R;
	}
	/*!\brief transform the turtle into the frame f,
	which is interpreted RELATIVE to the current frame
	(new = f * old) */
	void Transformation::appendTransformation(const Transformation& f){
#ifdef ORS_NO_DYNAMICS_IN_FRAMES
		pos += rot*f.pos;
		rot = rot*f.rot;
#else
		//Vector P(r*(s*f.p)); //relative offset in global coords
		//Vector V(r*(s*f.v)); //relative vel in global coords
		Matrix R;
		rot.getMatrix(R.p);
		Vector P(R*f.pos); //relative offset in global coords
		Vector V(R*f.vel); //relative vel in global coords
		Vector W(R*f.angvel); //relative ang vel in global coords
		pos += P;
		vel += angvel^P;
		vel += V;
		//a += b^P;
		//a += w^((w^P) + 2.*V);
		//a += r*(s*f.a);
		//b += w^W;
		//b += r*f.b;
		angvel += W;
		rot = rot*f.rot;
		//s*=f.s;
#endif
	}
	//! inverse transform (new = f^{-1} * old) or (old = f * new) 
	void Transformation::appendInvTransformation(const Transformation& f){
		//s/=f.s;
		rot=rot/f.rot;
		angvel-=rot*f.angvel;
		vel-=rot*f.vel;
		//v-=r*(s*f.v);
		//Vector P(r*(s*f.p)); //frame offset in global coords
		Vector P(rot*f.pos); //frame offset in global coords
		vel-=angvel^P;
		pos-=P;
	}
	//! new = old * f
	void Transformation::prependTransformation(const Transformation& f){
		Transformation newf;
		newf=f;
		newf.appendTransformation(*this);
		*this=newf;
	}
	//! new = old * f^{-1}
	void Transformation::prependInvTransformation(const Transformation& f){
		Transformation newf;
		newf.setInverse(f);
		newf.appendTransformation(*this);
		*this=newf;
	}
	//! this = f^{-1}
	void Transformation::setInverse(const Transformation& f){ setZero(); appendInvTransformation(f); }

	//! set double[4*4] to Transformation. Matrix needs to be orthogonal
	void Transformation::setAffineMatrix(const double *m){
		double M[9];
		uint i,j;
		for(i=0; i<3; ++i) 
			for(j=0; j<3; ++j) 
				M[i*3+j] = m[i*4+j];
		rot.setMatrix(M);                 // set 3x3 submatrix as rotation
		for(i=0; i<3; ++i) pos(i)=m[i*4+3];  // set last column as translation
	}

	//!  to = new * from
	void Transformation::setDifference(const Transformation& from,const Transformation& to){
		//s=to.s/from.s;
		rot=Quaternion()/from.rot *to.rot;
		angvel=from.rot/(to.angvel-from.angvel);
		/*v=(1./from.s) * (from.r/(to.v-from.v));
		v-=(1./from.s) * (from.r/(from.w^(to.p-from.p)));
		p=(1./from.s) * (from.r/(to.p-from.p));*/
		vel = from.rot/(to.vel-from.vel);
		vel-= from.rot/(from.angvel^(to.pos-from.pos));
		pos = from.rot/(to.pos-from.pos);
	}

	//! get the current position/orientation/scale in an OpenGL format matrix (of type double[16])
	double* Transformation::getAffineMatrix(double *m) const{
		double M[9]; rot.getMatrix(M);
		m[0] =M[0]; m[1] =M[1]; m[2] =M[2]; m[3] =pos(0);
		m[4] =M[3]; m[5] =M[4]; m[6] =M[5]; m[7] =pos(1);
		m[8] =M[6]; m[9] =M[7]; m[10]=M[8]; m[11]=pos(2);
		m[12]=0.;   m[13]=0.;   m[14]=0.;   m[15]=1.;
		return m;
	}

	//! get inverse OpenGL matrix for this frame (of type double[16])
	double* Transformation::getInverseAffineMatrix(double *m) const{
		double M[9]; rot.getMatrix(M);
		Vector pinv; pinv=rot/pos;
		m[0] =M[0]; m[1] =M[3]; m[2] =M[6]; m[3] =-pinv(0);
		m[4] =M[1]; m[5] =M[4]; m[6] =M[7]; m[7] =-pinv(1);
		m[8] =M[2]; m[9] =M[5]; m[10]=M[8]; m[11]=-pinv(2);
		m[12]=0.;   m[13]=0.;   m[14]=0.;   m[15]=1.;
		return m;
	}

	//! get the current position/orientation/scale in an OpenGL format matrix (of type double[16])
	double* Transformation::getAffineMatrixGL(double *m) const{
		double M[9]; rot.getMatrix(M);
		m[0]=M[0]; m[4]=M[1]; m[8] =M[2]; m[12]=pos(0);
		m[1]=M[3]; m[5]=M[4]; m[9] =M[5]; m[13]=pos(1);
		m[2]=M[6]; m[6]=M[7]; m[10]=M[8]; m[14]=pos(2);
		m[3]=0.;   m[7]=0.;   m[11]=0.;   m[15]=1.;
		return m;
	}

	//! get inverse OpenGL matrix for this frame (of type double[16]) */
	double* Transformation::getInverseAffineMatrixGL(double *m) const{
		double M[9]; rot.getMatrix(M);
		Vector pinv; pinv=rot/pos;
		m[0]=M[0]; m[4]=M[3]; m[8] =M[6]; m[12]=-pinv(0);
		m[1]=M[1]; m[5]=M[4]; m[9] =M[7]; m[13]=-pinv(1);
		m[2]=M[2]; m[6]=M[5]; m[10]=M[8]; m[14]=-pinv(2);
		m[3]=0.;   m[7]=0.;   m[11]=0.;   m[15]=1.;
		return m;
	}

	//! operator<<
	void Transformation::write(std::ostream& os) const{
		bool space=false;
		os <<"<";
		if(!pos.isZero()){ os <<"t" <<pos;  space=true; }
		if(!vel.isZero()){ if(space) os <<' ';  os <<"v" <<vel;  space=true; }
		if(!angvel.isZero()){ if(space) os <<' ';  os <<"w" <<angvel;  space=true; }
		if(!rot.isZero()){ if(space) os <<' ';  os <<"q" <<rot;  space=true; }
		//if(s!=1.) os <<" s(" <<s <<") ";
		os <<">";
	}
	//! operator>>
	void Transformation::read(std::istream& is){
		setZero();
		char c;
		double x[4];
		MT::skip(is," \n\r\t<");
		for(;;){
			is >>c;
			if(is.fail()) return; //EOF I guess
			//if(c==';') break;
			//if(c==',') is >>c;
			switch(c){
				//case '<': break; //do nothing -- assume this is an opening tag
			case 't': is>>"(">>x[0]>>x[1]>>x[2]>>")";	      addRelativeTranslation(x[0],x[1],x[2]); break;
			case 'q': is>>"(">>x[0]>>x[1]>>x[2]>>x[3]>>")"; addRelativeRotationQuat(x[0],x[1],x[2],x[3]); break;
			case 'r': is>>"(">>x[0]>>x[1]>>x[2]>>x[3]>>")"; addRelativeRotationRad(x[0],x[1],x[2],x[3]); break;
			case 'd': is>>"(">>x[0]>>x[1]>>x[2]>>x[3]>>")"; addRelativeRotationDeg(x[0],x[1],x[2],x[3]); break;
			case 'v': is>>"(">>x[0]>>x[1]>>x[2]>>")";	      addRelativeVelocity(x[0],x[1],x[2]); break;
			case 'w': is>>"(">>x[0]>>x[1]>>x[2]>>")";       addRelativeAngVelocityRad(x[0],x[1],x[2]); break;
				//case 's': is>>"(">>x[0]>>")";                   scale(x[0]); break;
			case '|': is.putback('<'); return;
			case '>': return; //those symbols finish the reading without error
			default: MT_MSG("unknown Transformation read tag: " <<c <<"abort reading this frame"); is.putback(c); return;
			}
			if(is.fail()) HALT("error reading '"<<c<<"' parameters in frame");
		}
		if(is.fail()) HALT ("could not read Transformation struct");
	}

}

//! use as similarity measure (distance = 1 - |scalarprod|)
double scalarProduct(const ors::Quaternion& a,const ors::Quaternion& b){
  return a.p[0]*b.p[0]+a.p[1]*b.p[1]+a.p[2]*b.p[2]+a.p[3]*b.p[3];
}

std::istream& operator>>(std::istream& is,ors::Vector& x)    { x.read(is); return is; }
std::istream& operator>>(std::istream& is,ors::Matrix& x)    { x.read(is); return is; }
std::istream& operator>>(std::istream& is,ors::Quaternion& x){ x.read(is); return is; }
std::istream& operator>>(std::istream& is,ors::Transformation& x)     { x.read(is); return is; }
std::ostream& operator<<(std::ostream& os,const ors::Vector& x)    { x.write(os); return os; }
std::ostream& operator<<(std::ostream& os,const ors::Matrix& x)    { x.write(os); return os; }
std::ostream& operator<<(std::ostream& os,const ors::Quaternion& x){ x.write(os); return os; }
std::ostream& operator<<(std::ostream& os,const ors::Transformation& x)     { x.write(os); return os; }